import matplotlib.pyplot as plt
import numpy as np
import math

#0阶滤波器，一阶信号，无测量噪声，检验截断误差

delta = 0.1                               #采样时间
T = 10                                    #总时间
A0 = 1
A1 = 2
t_0 = np.arange(0, T, 0.01)               #未采样前连续时间
Xk = [0]*len(t_0)                         #1阶真实信号
for i in range(0,len(t_0)):
    Xk[i] = A0 * t_0[i]+A1
k = list(range(1,int(T/delta)+1))         #采样次数
K = [0.0]*len(k)                          #滤波器的增益
wk = list(range(0,int(T/delta)))         #干扰值
x_measure = list(range(0,int(T/delta)))  #测量值
x_hat_prev = 0                              #设定初始的估计值
x_hat = [0]*len(k)                          #估计值
t = [0]*len(k)
e = [0]*len(k)                    #估计值的误差
ek = [0] * len(k)                  #截断误差
#计算测量值,此时无测量噪声
for i in range(0, len(k)):
    x_measure[i] = A0 * t[i] + A1
#计算估计值
for i in range(0, len(k)):
    t[i] = (K[i] - 1) * delta
    K[i] = 1 / k[i]
    x_hat[i] = x_hat_prev + K[i] * (x_measure[i] - x_hat_prev)
    x_hat_prev = x_hat[i]

#计算误差，对比估计误差与截断误差
for i in range(0, len(k)):
    e[i] = x_hat[i] - (A0 * t[i] + A1)
    ek[i] = (A1 * delta / 2) * (k[i] - 1)

plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t,x_measure,'-o',c = 'r',label = 'measures')
plt.plot(t,x_hat,'--',label = 'zero-order iterative least squares filter')
plt.plot(t_0, Xk, c="k",label = 'zero-order least squares filter',ls="--", lw=2)
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
#plt.plot(t,e,'-o',label = 'error between estimates and measures')
#plt.axhline(y=Xk-xhat, c="k",label = 'error between estimates and true',ls="--", lw=2)
plt.plot(t,e,'-o',label = 'error between estimates and true')
plt.plot(t,ek,'-o',label = 'error between estimates and true')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
#plt.plot(t,X_measure,'-o',c = 'r',label = 'measures')
plt.plot(t_0,Xk,label = 'true')
plt.plot(t,x_hat,label = 'estimates')
#plt.xlim(0,T)
#plt.ylim(0.8, 1.2)
plt.legend()
plt.show()